Note: This tutorial assumes you are comfortable with using roscpp, and have gone through the ROS Tutorials.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

マーカ: 基本立体を送る(C++)

Description: 基本立体をrvizに送るためのvisualization_msgs/Markerメッセージの使い方を紹介します

Tutorial Level: BEGINNER

Next Tutorial: Markers: Points and Lines

  Show EOL distros: 

イントロ

他のディスプレイと違い、Marker Displayは、rvizがデータを解釈すること以外について何も知らずに、データを可視化することができます。その代わり、初等的なオブジェクトは、visualization_msgs/Markerのメッセージを通して送られ、矢や箱や、円や、直線を描画することができます。

このチュートリアルは、いかにして基本立体(箱、球、シリンダ、矢) を送るかについて紹介します。新しいマーカを毎秒送り、常に異なった形にしていくプログラムを作成します。

スクラッチパッケージを作る

始める前に、パッケージパスのどこかにusing_markersと呼ばれるスクラッチパッケージを作成します。

catkin_create_pkg using_markers roscpp visualization_msgs

roscreate-pkg using_markers roscpp visualization_msgs

マーカを送る

コード

src/basic_shapes.cppに以下のものをコピー&ペーストしてください。

https://raw.github.com/ros-visualization/visualization_tutorials/hydro-devel/visualization_marker_tutorials/src/basic_shapes.cpp

  30 #include <ros/ros.h>
  31 #include <visualization_msgs/Marker.h>
  32 
  33 int main( int argc, char** argv )
  34 {
  35   ros::init(argc, argv, "basic_shapes");
  36   ros::NodeHandle n;
  37   ros::Rate r(1);
  38   ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
  39 
  40   // Set our initial shape type to be a cube
  41   uint32_t shape = visualization_msgs::Marker::CUBE;
  42 
  43   while (ros::ok())
  44   {
  45     visualization_msgs::Marker marker;
  46     // Set the frame ID and timestamp.  See the TF tutorials for information on these.
  47     marker.header.frame_id = "/my_frame";
  48     marker.header.stamp = ros::Time::now();
  49 
  50     // Set the namespace and id for this marker.  This serves to create a unique ID
  51     // Any marker sent with the same namespace and id will overwrite the old one
  52     marker.ns = "basic_shapes";
  53     marker.id = 0;
  54 
  55     // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
  56     marker.type = shape;
  57 
  58     // Set the marker action.  Options are ADD and DELETE
  59     marker.action = visualization_msgs::Marker::ADD;
  60 
  61     // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  62     marker.pose.position.x = 0;
  63     marker.pose.position.y = 0;
  64     marker.pose.position.z = 0;
  65     marker.pose.orientation.x = 0.0;
  66     marker.pose.orientation.y = 0.0;
  67     marker.pose.orientation.z = 0.0;
  68     marker.pose.orientation.w = 1.0;
  69 
  70     // Set the scale of the marker -- 1x1x1 here means 1m on a side
  71     marker.scale.x = 1.0;
  72     marker.scale.y = 1.0;
  73     marker.scale.z = 1.0;
  74 
  75     // Set the color -- be sure to set alpha to something non-zero!
  76     marker.color.r = 0.0f;
  77     marker.color.g = 1.0f;
  78     marker.color.b = 0.0f;
  79     marker.color.a = 1.0;
  80 
  81     marker.lifetime = ros::Duration();
  82 
  83     // Publish the marker
  84     marker_pub.publish(marker);
  85 
  86     // Cycle between different shapes
  87     switch (shape)
  88     {
  89     case visualization_msgs::Marker::CUBE:
  90       shape = visualization_msgs::Marker::SPHERE;
  91       break;
  92     case visualization_msgs::Marker::SPHERE:
  93       shape = visualization_msgs::Marker::ARROW;
  94       break;
  95     case visualization_msgs::Marker::ARROW:
  96       shape = visualization_msgs::Marker::CYLINDER;
  97       break;
  98     case visualization_msgs::Marker::CYLINDER:
  99       shape = visualization_msgs::Marker::CUBE;
 100       break;
 101     }
 102 
 103     r.sleep();
 104   }
 105 }

using_markersのパッケージの中のCMakeLists.txt`を編集してください。 以下を最終行に加えます:

rosbuild_add_executable(basic_shapes src/basic_shapes.cpp)

add_executable(basic_shapes src/basic_shapes.cpp)
target_link_libraries(basic_shapes ${catkin_LIBRARIES})

コードの解説

さて、少しずつコードを読み解いていきましょう。: <

  30 #include <ros/ros.h>
  31 #include <visualization_msgs/Marker.h>
  32 

今までにROSのインクルードを見たことはあるでしょう。他にvisualization_msgs/Markerからメッセージの定義もインクルードしておきます。

  33 int main( int argc, char** argv )
  34 {
  35   ros::init(argc, argv, "basic_shapes");
  36   ros::NodeHandle n;
  37   ros::Rate r(1);
  38   ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

これもおなじみの行ですね。ROSを初期化して、visualization_markerのtopicにros::Publisherを作成しています。

  41   uint32_t shape = visualization_msgs::Marker::CUBE;

ここで、何の形の立体を描画するかを保存しておくint型の変数を用意します。使う予定の4つの立体は、同じようにしてvisualization_msgs/Markerのメッセージを利用するので、異なる立体を表す形のタイプを変えるのみで描画する立体を変えることができます。

  43   while (ros::ok())
  44   {
  45     visualization_msgs::Marker marker;
  46     // Set the frame ID and timestamp.  See the TF tutorials for information on these.
  47     marker.header.frame_id = "/my_frame";
  48     marker.header.stamp = ros::Time::now();

このプログラムの大事な部分がここから始まります。まず、visualization_msgs/Markerを作り、必要な設定を施します。headerはここでは、tf tutorialsに触れたことがあれば、よく知っていると思われるroslib/Headerを指します。例としてframe_id/my_frameに指定します。実行されているシステムの中では、markerの形を解釈させたいものに依ってframeを指定する必要があります。

  52     marker.ns = "basic_shapes";
  53     marker.id = 0;

名前空間(ns)とidはこのマーカ特有の名前を作るために使用します。もし、マーカのメッセージが同じnsidで受け取ったなら、古いものを新しいマーカで置き換えます。

  56     marker.type = shape;

このtypeの変数は、どのような種類のマーカを送るかを特定するためのものです。利用できる型はvisualization_msgs/Markerのメッセージの中に列挙されています。ここでは、ループのたびに変わるshape変数をセットします。

  59     marker.action = visualization_msgs::Marker::ADD;

action変数は、マーカをどのようにするかを指定します。オプションは、visualization_msgs::Marker::ADD and visualization_msgs::Marker::DELETEです。ADDは、不適切な名称で、実際は、"create or modify"(作成または修正)を意味します。

New in Indigo A new action has been added to delete all markers in the particular Rviz display, regardless of ID or namespace. The value is 3 and in future ROS version the message will change to have value visualization_msgs::Marker::DELETEALL.

  62     marker.pose.position.x = 0;
  63     marker.pose.position.y = 0;
  64     marker.pose.position.z = 0;
  65     marker.pose.orientation.x = 0.0;
  66     marker.pose.orientation.y = 0.0;
  67     marker.pose.orientation.z = 0.0;
  68     marker.pose.orientation.w = 1.0;

ここで、マーカのポーズを設定します。geometry_msgs/Poseのメッセージは、位置を特定するgeometry_msgs/Vector3と角度を指定するgeometry_msgs/Quaternionで構成されます。原点に位置を設定し、基本姿勢に角度を設定しています。(注意wのみ1にする).

  71     marker.scale.x = 1.0;
  72     marker.scale.y = 1.0;
  73     marker.scale.z = 1.0;

マーカのスケールを設定します。基本立体を1のスケールに指定したときはサイドが1mあることを示します。

  76     marker.color.r = 0.0f;
  77     marker.color.g = 1.0f;
  78     marker.color.b = 0.0f;
  79     marker.color.a = 1.0;

マーカの色をstd_msgs/ColorRGBAとして指定します。どのメンバも、0から1の間の数であるべきで、(a)の値が0であることは、完全に透明であることを示し、1で不透明であることを示します。

  81     marker.lifetime = ros::Duration();

lifetimeの変数は、自動的に削除されるまでマーカがあるべきかを特定します。ros::Duration()の値は、いつまでも消えないことを意味します。

もしlifetimeが期限に到達する前に、新しいマーカのメッセージが来た場合は、lifetimeは新しいマーカのメッセージの値にリセットされます。

  84     marker_pub.publish(marker);

これで、マーカが配信されました。

  87     switch (shape)
  88     {
  89     case visualization_msgs::Marker::CUBE:
  90       shape = visualization_msgs::Marker::SPHERE;
  91       break;
  92     case visualization_msgs::Marker::SPHERE:
  93       shape = visualization_msgs::Marker::ARROW;
  94       break;
  95     case visualization_msgs::Marker::ARROW:
  96       shape = visualization_msgs::Marker::CYLINDER;
  97       break;
  98     case visualization_msgs::Marker::CYLINDER:
  99       shape = visualization_msgs::Marker::CUBE;
 100       break;
 101     }

このコードは一つのマーカのメッセージを配信するだけで、すべての4つの形を見ることができるものです。現在の形に基づいて、次の形を何にするかを決定しています。

 103     r.sleep();
 104   }

sleepとloop backをします。

コードをビルドする

コードを以下のコマンドでビルドできます。:

$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE%
$ catkin_make    

rosmake using_markers

コードを実行します

コードを以下のコマンドとともに実行できます。:

rosrun using_markers basic_shapes

マーカを可視化する

マーカを配信しているので、rvizにそれらを見えるように設定する必要があります。まず, rivzがビルドされていることを確認し:

rosmake rviz

rvizを実行します:

rosrun using_markers basic_shapes
rosrun rviz rviz

もし、rvizを今までに使ったことがなければ、User's Guideを見て、はじめてください。

まず初めにすることは、tfのtransformが準備できていないため、Fixed Framesを上でマーカをセットしたframe(ここでは、/myframe)に設定することです。これを実行するために、Fixed Frameの領域に"/my_frame"を設定してください。

まず初めにすることは、tfのtransformが準備できていないため、Fixed Framesを上でマーカをセットしたframe(ここでは、/myframe)に設定することです。これを実行するために、Fixed Frameの領域に"/my_frame"を設定してください。

まず初めにすることは、tfのtransformが準備できていないため、Fixed Framesを上でマーカをセットしたframe(ここでは、/myframe)に設定することです。それをするために、Fixed Frameの変数を見るために"Displays"エリアの".Global Options" を広げる必要があります。 "/my_frame"をタイプしてください。

まず初めにすることは、tfのtransformが準備できていないため、Fixed and Target Framesを上でマーカをセットしたframe(ここでは、/myframe)に設定することです。それをするために、Fixed Frameの変数を見るために"Displays"エリアの".Global Options" を広げる必要があります。 "/my_frame"をタイプしてください。

まず初めにすることは、tfのtransformが準備できていないため、Fixed and Target Framesを上でマーカをセットしたframe(ここでは、/myframe)に設定することです。それをするために、"{ Fixed, Target } Frame"の両方を見るために"Displays"エリアの".Global Options" を広げる必要があります。 "/my_frame"をタイプしてください。

次に、Markers displayを追加します。デフォルトで指定されているvisualization_markerは配信されているものと同じであることに注意してください。

そうすると、以下のような毎秒形が変わるものが見えるでしょう。 Basic Shapes

さらに詳しく

ここで紹介した以外のマーカの異なる型についての情報は、Markers Display Pageで参照してください。

Wiki: ja/rviz/Tutorials/Markers: Basic Shapes (last edited 2014-10-21 04:02:10 by Moirai)